home *** CD-ROM | disk | FTP | other *** search
/ Disc to the Future 2 / Disc to the Future Part II Programmer's Reference (Wayzata Technology)(6013)(1992).bin / MAC / THINKC / 4_0 / ORBMECHD / LAMBERT.C < prev    next >
Text File  |  1991-02-18  |  10KB  |  412 lines
  1. /********************************************************************
  2.     Karl Dishaw   8 Feb 91
  3.     Program for solution of Lambert's Problem using the Improved 
  4.     Gauss' Method.  Given the two radii, the angle between them, 
  5.     and the time for the transfer, the semimajor axis and parameter 
  6.     of an orbit is found.  
  7.  
  8.     Basic algorithm from Battin, Mathematics and Methods of Astrodynamics
  9.     Method in section 7.5, pp325-339.  Implemented in C by Karl Dishaw.
  10. **********************************************************************/
  11.     
  12. #include    "orbmech.h"
  13. #include    "SANE.h"
  14. #include    "math.h"
  15. #include    "convert.h"
  16. #define        LAMBERTREC_SIZE        60
  17.  
  18. /* globals */
  19.  
  20. extern    DialogPtr        LambertDia, HohmannDia, HillsDia, KeplerDia, whichDialog, gHelpDia;
  21. extern    EventRecord        gEvent;
  22. extern    TEHandle        TEH;
  23. extern    decform            gdecform;
  24. extern    int                dirty;                
  25. extern    Boolean            gifWNE;    
  26.  
  27. struct    ParamRec
  28.     {
  29.     extended    value;
  30.     Str255        name;
  31.     };
  32.     
  33. extern     struct    ParamRec    gParameter;
  34.  
  35. struct    LambertRec
  36.     {
  37.     extended         flight_time, rad_one, rad_two, theta;
  38.     extended         a, e;  
  39.     } ;
  40.     
  41. typedef        struct    LambertRec    *LambertPtr;
  42.     
  43.  
  44. /*****************************************/
  45.  
  46. Lambert()
  47. {
  48.     Handle        inputH, itemHandle;
  49.     Str255        r1Str, r2Str, ftStr, thStr, aStr, eStr;
  50.     int            itemType;
  51.     Rect        itemRect;
  52.     Boolean        dont_panic = 1, miFlag = 0, auFlag = 0;
  53.     decimal        result;
  54.     
  55.     ControlHandle    kmSecBut, miHrBut, radBut;
  56.  
  57.     struct        LambertRec    *data2;
  58.     
  59.     Ptr            temp;
  60.  
  61.     Str255        q, w, e, r, t, y, u, i, o, p;    
  62.     
  63.         /*  set up structure to hold data  */
  64.     temp = NewPtr( LAMBERTREC_SIZE );
  65.     data2 = ( LambertPtr) temp;
  66.  
  67.         /*  initialize strings for record file  */
  68.     pStrCopy("\p\rTime Specific Transfer ", q);
  69.     pStrCopy("\p\r     radius 1:   ", w);
  70.     pStrCopy("\p\r     radius 2:   ", e);
  71.     pStrCopy("\p\r     angle:      ", r);
  72.     pStrCopy("\p\r     time:       ", t);
  73.     pStrCopy("\p\r     primary:    ", y);
  74.     pStrCopy("\p\r\rTransfer orbit components ", u);
  75.     pStrCopy("\p\r     semi-major axis:  ", i);
  76.     pStrCopy("\p\r     eccentricity:     ", o);
  77.     pStrCopy("\p\r\r", p);
  78.  
  79.         /*  read data from dialogue box  */
  80.     GetDItem( LambertDia, TIME, &itemType, &inputH, &itemRect);
  81.     GetIText( inputH, &ftStr );
  82.     Pstr_to_extended( &ftStr, &data2->flight_time, &dont_panic );
  83.     
  84.     GetDItem( LambertDia, RAD_1, &itemType, &inputH, &itemRect);
  85.     GetIText( inputH, &r1Str );
  86.     Pstr_to_extended( &r1Str, &data2->rad_one, &dont_panic );
  87.     
  88.     GetDItem( LambertDia, RAD_2, &itemType, &inputH, &itemRect);
  89.     GetIText( inputH, &r2Str );
  90.     Pstr_to_extended( &r2Str, &data2->rad_two, &dont_panic );
  91.     
  92.     GetDItem( LambertDia, THETA, &itemType, &inputH, &itemRect);
  93.     GetIText( inputH, &thStr );
  94.     Pstr_to_extended( &thStr, &data2->theta, &dont_panic );
  95.     
  96.         /*  check radio buttons for units  */
  97.     GetDItem ( LambertDia, KM_SEC, &itemType, &kmSecBut, &itemRect);
  98.     GetDItem ( LambertDia, MI_HR, &itemType, &miHrBut, &itemRect);
  99.     GetDItem ( LambertDia, RADIANS, &itemType, &radBut, &itemRect);
  100.     
  101.     if ( dont_panic )   /*  if data read successfully  */
  102.     {
  103.     
  104.         /*  convert units  */
  105.     if ( ! GetCtlValue ( kmSecBut ) )
  106.         if ( GetCtlValue ( miHrBut ) ) {
  107.             data2->rad_one *=  MI_CONV;
  108.             data2->rad_two *=  MI_CONV;
  109.             data2->flight_time *= HR_CONV;
  110.             miFlag = 1;
  111.         }
  112.         else {
  113.             data2->rad_one *=  AU_CONV;
  114.             data2->rad_two *=  AU_CONV;
  115.             data2->flight_time *= YR_CONV;
  116.             auFlag = 1;
  117.         }    
  118.     
  119.     if ( ! GetCtlValue ( radBut ) )
  120.         data2->theta /=  DEG_CONV;
  121.     
  122.         /*  write problem starting conditions to file  */
  123.     TEInsert( q+1, *q, TEH );
  124.     TEInsert( w+1, *w, TEH );
  125.     TEInsert( r1Str+1, *r1Str, TEH );
  126.     TEInsert( e+1, *e, TEH );
  127.     TEInsert( r2Str+1, *r2Str, TEH );
  128.     TEInsert( r+1, *r, TEH );
  129.     TEInsert( thStr+1, *thStr, TEH );
  130.     TEInsert( t+1, *t, TEH );
  131.     TEInsert( ftStr+1, *ftStr, TEH );
  132.     TEInsert( y+1, *y, TEH );
  133.     TEInsert( gParameter.name+1, *gParameter.name, TEH );
  134.  
  135.         /*  perform calculations  */
  136.     lambert_calculations( data2 );
  137.     
  138.         /*  convert units  */
  139.     if ( miFlag ) 
  140.         data2->a /=  MI_CONV;
  141.     
  142.     if ( auFlag )  
  143.         data2->a /=  AU_CONV;
  144.     
  145.         /* convert data format  */
  146.     if ( data2->a > 9999.0  || data2->a <  .000001 ) 
  147.         gdecform.style = FLOATDECIMAL;
  148.     else
  149.         gdecform.style = FIXEDDECIMAL;
  150.     
  151.     num2dec( &gdecform, data2->a, &result );    
  152.     dec2str( &gdecform, &result, aStr );
  153.  
  154.     if ( data2->e > 9999.0  || data2->e <  .000001 ) 
  155.         gdecform.style = FLOATDECIMAL;
  156.     else
  157.         gdecform.style = FIXEDDECIMAL;
  158.     
  159.     num2dec( &gdecform, data2->e, &result );    
  160.     dec2str( &gdecform, &result, eStr );    
  161.  
  162.         /*  write results to file  */
  163.     TEInsert( u+1, *u, TEH );
  164.     TEInsert( i+1, *i, TEH );
  165.     TEInsert( aStr+1, *aStr, TEH );
  166.     TEInsert( o+1, *o, TEH );    
  167.     TEInsert( eStr+1, *eStr, TEH );
  168.     TEInsert( p+1, *p, TEH );
  169.  
  170.     dirty = 1;
  171.     
  172.         /*  write results to dialogue box  */
  173.     GetDItem( LambertDia, A, &itemType, &itemHandle, &itemRect);
  174.     SetIText( itemHandle, aStr );
  175.     
  176.     GetDItem( LambertDia, E, &itemType, &itemHandle, &itemRect);
  177.     SetIText( itemHandle, eStr );
  178.  
  179.     ShowSelect();
  180.     
  181.     }
  182. }
  183.  
  184. /**************************************/
  185.  
  186.  
  187. lambert_calculations( data2 )
  188. struct    LambertRec    *data2;
  189. {
  190.     extended mu, deltat, r1, r2, theta, rop, m, l, x;
  191.     extended  epsilon, omegaf, zeta, h1, h2, b, u, k;
  192.     extended y, t, a, e2, e, tva, tvb;
  193.     int converged, la, lb;
  194.     Str255    warn;
  195.     
  196.     extended cfa(), cfb();
  197.  
  198.     mu = gParameter.value;    /* km^3 / sec^2 */
  199.     deltat = data2->flight_time;    /* transfer time in seconds */
  200.     r1 = data2->rad_one;        /* radii of starting and destination points in km */
  201.     r2 = data2->rad_two;
  202.     theta = data2->theta;    /* angle between ra and rb in radians */
  203.     
  204.     pStrCopy("\p\r\rSTOP!  Calculation interrupted, results invalid", warn);
  205.  
  206.     epsilon = r2 / r1 - 1.0;
  207.         /*  7.56  */
  208.     omegaf = epsilon * epsilon / 4.0 / ( sqrt( r2 / r1 ) 
  209.                     + r2 / r1 * ( 2 + sqrt( r2 / r1 ) ) );
  210.         /*  7.57  */
  211.     tva = cos( theta / 4.0 ) * cos( theta / 4.0 ) + omegaf;
  212.     tvb = sin( theta / 4.0 ) * sin( theta / 4.0 ) + omegaf;
  213.         /*  temporary variables  */
  214.     rop = sqrt( r1 * r2 ) * tva;
  215.         /*  7.102  */
  216.     m = mu * deltat * deltat / ( 8.0 * rop * rop * rop );
  217.         /*  7.89 (3)  */
  218.     
  219.     if ( theta < 3.14159 )  {
  220.         l = tvb / ( tvb + cos( theta / 2.0 ) );
  221.     }
  222.     else  {
  223.         l = ( tva - cos( theta / 2.0 ) ) / tva;
  224.     }
  225.         /*  7.101  */
  226.             
  227.     x = l;
  228.     
  229.     converged = 0;
  230.     
  231.     while ( !converged ) {
  232.     
  233.         zeta = cfa( x );
  234.             /*  7.121  */
  235.         
  236.         h1 = ( l + x ) * ( l + x ) * ( 1.0 + 3.0 * x + zeta )
  237.                 / ( ( 1 + 2.0 * x + l ) * ( 4.0 * x + zeta * ( 3.0 + x )));
  238.             /*  7.111  */
  239.         h2 = m * ( x - l + zeta ) / ( 1.0 + 2.0 * x + l )
  240.                 / ( 4.0 * x + zeta * ( 3.0 + x ) ) ;
  241.             /*  7.112  */
  242.         b = 27.0 * h2 / ( 4.0 * ( 1.0 + h1 ) * ( 1.0 + h1 ) * ( 1.0 + h1 ));
  243.             /*  7.123 (2)  */
  244.         u = b / 2.0 / ( sqrt( 1.0 + b ) + 1.0 );
  245.             /*  7.123 (1)  */
  246.         k = cfb( u );
  247.             /*  7.125  */
  248.         y = ( 1.0 + h1 ) / 3.0 * ( 2.0 + sqrt( 1.0 + b ) 
  249.                 / ( 1.0 + 2.0 * u * k * k ) );
  250.             /*  7.124  */
  251.         t = sqrt( ( ( 1.0 - l ) / 2.0 ) * ( ( 1.0 - l ) / 2.0 ) 
  252.                 + m / y / y ) - ( 1.0 + l ) / 2.0;
  253.             /*  7.113  */
  254.  
  255.         converged = checkConvergence( x, t);
  256.         
  257.         if ( !converged ) {
  258.         
  259.             if ( gifWNE )
  260.                 WaitNextEvent( everyEvent, &gEvent, MIN_SLEEP, NIL_MOUSE_REGION );
  261.             else
  262.             {
  263.                 SystemTask();
  264.                 GetNextEvent( everyEvent, &gEvent );
  265.             }
  266.         
  267.             if ( (gEvent.modifiers & cmdKey) != 0  && ( ( gEvent.what == keyDown )
  268.                 || ( gEvent.what == autoKey ) ) && ( gEvent.message & charCodeMask )
  269.                 == POINT )
  270.                 {
  271.                     StopAlert( INTERRUPT, NIL_POINTER );
  272.                     TEInsert( warn+1, *warn, TEH );
  273.                     converged = 1;
  274.                 }
  275.             }
  276.             
  277.         x = t;
  278.         
  279.         }
  280.  
  281.     a = mu * deltat * deltat / ( 16.0 * r1 * r2 * tva * tva * x * y * y );
  282.         /*  7.103  */
  283.             
  284.     e2 = ( epsilon * epsilon + 4.0 * r2 / r1 * sin( theta / 2.0 ) 
  285.             * sin( theta / 2.0 ) * (( l - x ) / ( l + x )) 
  286.             * (( l - x ) / ( l + x ))) / ( epsilon * epsilon + 
  287.             4.0 * r2 / r1 * sin( theta / 2.0 ) * sin( theta / 2.0 ) );
  288.         /*  7.106  */
  289.             
  290.     e = sqrt( e2 );
  291.  
  292.     data2->a = a;            /* pass results back */
  293.     data2->e = e;
  294. }
  295.  
  296.  
  297. /**************************************/
  298.  
  299. extended     cfa(x)        /* top-down calculation of continued fraction */
  300. extended     x;
  301. {
  302.  
  303.             /*  reference Battin p311, pp335-7  */
  304.             
  305.     extended delta, deltaprev, sum, zeta;
  306.     extended u, gamma, eta;
  307.     int i, converged;
  308.  
  309.     eta = x / ( ( sqrt( 1.0 + x ) + 1.0 ) * (sqrt(1.0 + x ) + 1.0 ));
  310.     
  311.     i = 1;
  312.     deltaprev = 1.0;
  313.     sum = 1.0;
  314.     u = 1.0;
  315.     converged = 0;
  316.  
  317.     while(!converged){
  318.  
  319.         gamma = - (i + 3.0) * (i + 3.0) / ( 2.0 * i + 5.0) / ( 2.0 * i + 7.0);
  320.         delta = 1.0 / ( 1.0 - gamma * eta * deltaprev);
  321.         u *= delta - 1.0;
  322.         sum += u;
  323.  
  324.         converged = checkConvergence( u, 0.0 );
  325.  
  326.         deltaprev = delta;
  327.         i++;
  328.         
  329.         }
  330.         
  331.     zeta = 8.0 * ( sqrt( 1.0 + x ) + 1.0 ) / ( 3.0 + 1.0 / 
  332.                 ( 5.0 + eta + 9.0 * eta / 7.0 / sum ) );
  333.  
  334.     return ( zeta);
  335.     }
  336.  
  337. /**************************************/
  338.  
  339.  
  340. extended     cfb(z)        /* top-down calculation of continued fraction */
  341. extended     z;
  342. {
  343.  
  344.             /*  reference Battin p311, p339  */
  345.             
  346.     double m, n, gamma, u, delta, sum, deltaprev, k;
  347.     int odd, i, converged, j;
  348.  
  349.     odd = 1;
  350.     i = 1;
  351.     u = 1.0;
  352.     sum = 1.0;
  353.     deltaprev = 1.0;
  354.     converged = 0;
  355.  
  356.     while(!converged){
  357.  
  358.         if(odd){
  359.             j = (i - 1) / 2;
  360.             m = -2.0 * ( 3.0 * j + 2.0) * ( 6.0 * j + 1.0 );
  361.             n = 9.0 * (4.0 * j + 1.0) * (4.0 * j + 3.0);
  362.             gamma = m / n;
  363.             }
  364.         else{
  365.             j = i / 2;
  366.             m = - 2.0 * (3.0 * j + 1.0) * ( 6.0 * j - 1.0);
  367.             n = 9.0 * (4.0 * j - 1.0) * ( 4.0 * j + 1.0);
  368.             gamma = m / n;
  369.             }
  370.  
  371.         delta = 1.0 / ( 1.0 - gamma * z * deltaprev);
  372.         u *= delta - 1.0;
  373.         sum += u;
  374.         
  375.         converged = checkConvergence( u, 0.0 );
  376.  
  377.         deltaprev = delta;
  378.         
  379.         i++;
  380.         odd = !odd;
  381.         }
  382.         
  383.     k = sum / 3.0;
  384.  
  385.     return (k);
  386. }
  387.  
  388.  
  389. /**********************************************/
  390.  
  391. checkConvergence( a, b)
  392. extended    a, b;
  393. {
  394.             /*  check whether two numbers are effectively equal  */
  395.     Boolean        success = 0, flag1, flag2;
  396.     extended    limit = 1.0e-6, diff, var, zero = 0.0;
  397.     
  398.     diff = a - b;
  399.     
  400.     var = diff / a;
  401.     
  402.     if ( b == 0.0 ) 
  403.         var = a;
  404.     
  405.     flag1 = ( var >= zero ) && ( var < limit);
  406.     flag2 = ( var < zero ) && ( var > -limit);
  407.     
  408.     success = flag1 || flag2;
  409.     
  410.     return ( success );
  411. }
  412.